#include "MultiRobotSampler.h"
#include <boost/foreach.hpp>

MultiRobotSampler::MultiRobotSampler(vector<Polygon_2> &robot_polygons, int robot_num, Room &room, MultiRobotCollisionDetector &mr_collision_detector) :
    m_robot_polygons(robot_polygons), m_robot_num(robot_num), m_room(room), m_mr_collision_detector(mr_collision_detector)
{
    vector<CollisionDetector*>::const_iterator iter = m_mr_collision_detector.begin();
    BOOST_FOREACH(Polygon_2& robot, m_robot_polygons)
    {
        Sampler* sampler = new Sampler(robot, m_room, *iter);
        ++iter;

        m_samplers.push_back(sampler);
    }
}


MultiRobotSampler::~MultiRobotSampler(void)
{
    for (int index = 0; index < m_robot_num; ++index)
    {
        delete m_samplers[index];
    }
    m_samplers.clear();
}

vector<Conf> MultiRobotSampler::generate_sample()
{
    while (true)
    {
        vector<Conf> p = generate_no_collision_test();
        //make sure they don't overlap
        if (m_mr_collision_detector.valid_conf(p))
        {
            return p;
        }
    }
}

vector<Conf> MultiRobotSampler::generate_no_collision_test()
{
    vector<Conf> sample;
    //generate sample for each robot
    for (int index = 0; index < m_robot_num; ++index)
    {
        sample.push_back(m_samplers[index]->generate_sample());
    }
    return sample;
}
